/* This code is subject to the terms of the Mozilla Public License, v.2.0. http://mozilla.org/MPL/2.0/. */
#include "SimpleCameraCalibration.h"

using std::vector;

namespace {
	template <typename T>
	double distance(const point<T>& a, const point<T>& b)
	{
		return sqrt((double)a.squared_distance(b));
	}

	double edge_to_anchor_ratio(int size, int anchor_size, int edge_distance)
	{
		point<int> o0 = {anchor_size, anchor_size};
		point<int> o4 = {size / 2, edge_distance}; // 3  = edge distance
		point<int> o1 = {size-anchor_size, anchor_size};
		point<int> omid = {size / 2, anchor_size};
		return distance(o4, omid) / distance(o0, o1);
	}

	bool get_distortion_factor(vector<double>& ratios, const point<int>& observed, const point<double>& expected, const point<int>& start, const point<int>& end)
	{
		/* distortion_factor is generated by distance from the target_ratio.
		   The expected calculation is in edge_to_anchor_ratio()
		*/
		if (!observed)
			return false;

		double rat = distance(observed.to_float(), expected) / distance(start, end);
		ratios.push_back(rat);
		return true;
	}

}

SimpleCameraCalibration::SimpleCameraCalibration()
    : _size(1024)
    , _targetRatio(edge_to_anchor_ratio(_size, 30, 3))
{}

double SimpleCameraCalibration::calculate_distortion_factor(const Corners& corners, const Midpoints& mps, const vector<point<int>>& edges)
{
	std::vector<double> ratios;
	get_distortion_factor(ratios, edges[0], mps.top(), corners.top_left(), corners.top_right());
	get_distortion_factor(ratios, edges[1], mps.right(), corners.top_right(), corners.bottom_right());
	get_distortion_factor(ratios, edges[2], mps.bottom(), corners.bottom_right(), corners.bottom_left());
	get_distortion_factor(ratios, edges[3], mps.left(), corners.bottom_left(), corners.top_left());

	if (ratios.size() == 0)
		return {};

	double total = 0;
	for (double& r : ratios)
		total += r;

	double smallest = _targetRatio - (total / ratios.size());
	for (double& r : ratios)
	{
		double dist = _targetRatio - r;
		if (fabs(dist) < fabs(smallest))
			smallest = dist;
	}
	return smallest;
}

